#include "Particle.h"
#include <cmath>
#include <iostream>

#include "Grid.h"

extern Particle** particles;
extern int totalNumberOfParticles;
extern float springCoefficient;
extern float dampingCoefficient;
extern float particleRadius;
extern float worldSize;

extern float voxelSize;
extern int gridLength;
extern float gridMinimumPosition;

int Particle::indexCount = 0;

Particle::Particle(float* position, float mass) {
	this->position[0] = position[0];
	this->position[1] = position[1];
	this->position[2] = position[2];
	this->mass = mass;
	velocity = {0.0f, 0.0f, 0.0f};
}

float* Particle::getPosition() {
	return position;
}

float* Particle::getVelocity() {
	return velocity;
}

float* Particle::getForce() {
	return force;
}

void Particle::populateArray() {
	particleIndex = indexCount;
	particles[indexCount] = this;
	indexCount++;
}

void Particle::applyRotation(float* rotationMatrix, float* originalRelativePos, float* bodyPosition) {

	position[0] = originalRelativePos[0]*rotationMatrix[0] + 
		originalRelativePos[1]*rotationMatrix[1] + 
		originalRelativePos[2]*rotationMatrix[2];

	position[1] = originalRelativePos[0]*rotationMatrix[3] + 
		originalRelativePos[1]*rotationMatrix[4] + 
		originalRelativePos[2]*rotationMatrix[5];

	position[2] = originalRelativePos[0]*rotationMatrix[6] + 
		originalRelativePos[1]*rotationMatrix[7] + 
		originalRelativePos[2]*rotationMatrix[8];

	position[0] = position[0] + bodyPosition[0];
	position[1] = position[1] + bodyPosition[1];
	position[2] = position[2] + bodyPosition[2];
}

float* Particle::calculateForces() {
	force = {0.0f, 0.0f, 0.0f};
//	calculateCollisionForces();
	calculateCollisionForcesWithGrid();
	calculateBoundaryForces();
	return force;
}

void Particle::updateVelocity(float* bodyPosition, float* bodyVelocity, float* bodyAngularVelocity) {
	velocity = {0.0f, 0.0f, 0.0f};

	float scalar = sqrt(bodyAngularVelocity[0]*bodyAngularVelocity[0] + 
				bodyAngularVelocity[1]*bodyAngularVelocity[1] +
				bodyAngularVelocity[2]*bodyAngularVelocity[2]);

	if (scalar > 0.0f) {
		scalar *= scalar;
		float relativePosition[3] = {position[0]-bodyPosition[0],
			position[1]-bodyPosition[1],
			position[2]-bodyPosition[2]};

		scalar = (bodyAngularVelocity[0]*relativePosition[0] +
				bodyAngularVelocity[1]*relativePosition[1] +
				bodyAngularVelocity[2]*relativePosition[2]) / scalar;

		float term[3];
		term[0] = relativePosition[0] - bodyAngularVelocity[0]*scalar;
		term[1] = relativePosition[1] - bodyAngularVelocity[1]*scalar;
		term[2] = relativePosition[2] - bodyAngularVelocity[2]*scalar;

		velocity[0] = (bodyAngularVelocity[1]*term[2] - bodyAngularVelocity[2]*term[1]);
		velocity[1] = (bodyAngularVelocity[2]*term[0] - bodyAngularVelocity[0]*term[2]);
		velocity[2] = (bodyAngularVelocity[0]*term[1] - bodyAngularVelocity[1]*term[0]);
	}
	velocity[0] += bodyVelocity[0];
	velocity[1] += bodyVelocity[1];
	velocity[2] += bodyVelocity[2];
}

void Particle::calculateCollisionForces() {

	for (int j=0; j<totalNumberOfParticles; j++) {
		if (j != this->particleIndex) {

			float* jPos = particles[j]->getPosition();

			float distance[3];
			distance[0] = jPos[0] - position[0];
			distance[1] = jPos[1] - position[1];
			distance[2] = jPos[2] - position[2];

			float absDistance = sqrt(distance[0]*distance[0] +
					distance[1]*distance[1] +
					distance[2]*distance[2]);

			if (absDistance + 0.00001f < (2.0f * particleRadius)) {
				force[0] = force[0] - springCoefficient*(2.0f*particleRadius - absDistance)*(distance[0]/absDistance);
				force[1] = force[1] - springCoefficient*(2.0f*particleRadius - absDistance)*(distance[1]/absDistance);
				force[2] = force[2] - springCoefficient*(2.0f*particleRadius - absDistance)*(distance[2]/absDistance);

				float* jVel = particles[j]->getVelocity();
				float relativeVelocity[3];
				relativeVelocity[0] = jVel[0] - velocity[0];
				relativeVelocity[1] = jVel[1] - velocity[1];
				relativeVelocity[2] = jVel[2] - velocity[2];

				force[0] += dampingCoefficient*relativeVelocity[0];
				force[1] += dampingCoefficient*relativeVelocity[1];
				force[2] += dampingCoefficient*relativeVelocity[2];
			}
		}
	}
}

void Particle::calculateCollisionForcesWithGrid() {
	if (isValidIndex(gridIndex)) {
		int* neighborParticleIndices = getNeighborParticleIndices(gridIndex);

		for (int i=0; i<particlesPerVoxel*27; i++) {
			int neighborIndex = neighborParticleIndices[i];
			if (neighborIndex != -1 && neighborIndex != this->particleIndex) {

				Particle* neighbor = particles[neighborIndex];
				float* jPos = neighbor->getPosition();

				float distance[3];
				distance[0] = jPos[0] - position[0];
				distance[1] = jPos[1] - position[1];
				distance[2] = jPos[2] - position[2];

				float absDistance = sqrt(distance[0]*distance[0] +
						distance[1]*distance[1] +
						distance[2]*distance[2]);

				if (absDistance + 0.00001f < (2.0f * particleRadius)) {
					force[0] = force[0] - springCoefficient*(2.0f*particleRadius - absDistance)*(distance[0]/absDistance);
					force[1] = force[1] - springCoefficient*(2.0f*particleRadius - absDistance)*(distance[1]/absDistance);
					force[2] = force[2] - springCoefficient*(2.0f*particleRadius - absDistance)*(distance[2]/absDistance);

					float* jVel = neighbor->getVelocity();
					float relativeVelocity[3];
					relativeVelocity[0] = jVel[0] - velocity[0];
					relativeVelocity[1] = jVel[1] - velocity[1];
					relativeVelocity[2] = jVel[2] - velocity[2];

					force[0] += dampingCoefficient*relativeVelocity[0];
					force[1] += dampingCoefficient*relativeVelocity[1];
					force[2] += dampingCoefficient*relativeVelocity[2];
				}
			}
		}
		delete neighborParticleIndices;
	}
}


void Particle::calculateBoundaryForces() {

	bool collisionOccured = false;

	// Ground collision
	if (position[1]-particleRadius < 0.0f) {
		collisionOccured = true;
		force[1] += springCoefficient*(particleRadius-position[1]);
	}
	
	// X-axis Wall Collision
	if (position[0]-particleRadius < -worldSize) { 
		collisionOccured = true;
		force[0] += springCoefficient*(-worldSize - position[0] + particleRadius);
	} else if (position[0]+particleRadius > worldSize) {
		collisionOccured = true;
		force[0] += springCoefficient*(worldSize - position[0] - particleRadius);
	}

	// Z-axis Wall Collision
	if (position[2]-particleRadius < -worldSize) {
		collisionOccured = true;
		force[2] += springCoefficient*(-worldSize - position[2] + particleRadius);
	} else if (position[2]+particleRadius > worldSize) {
		collisionOccured = true;
		force[2] += springCoefficient*(worldSize - position[2] - particleRadius);
	}

	// Damping
	if (collisionOccured) {
		force[0] -= dampingCoefficient*velocity[0];
		force[1] -= dampingCoefficient*velocity[1];
		force[2] -= dampingCoefficient*velocity[2];
	}
}

void Particle::reset(float* oldBodyPos, float* newBodyPos) {
	position[0] += newBodyPos[0] - oldBodyPos[0];
	position[1] += newBodyPos[1] - oldBodyPos[1];
	position[2] += newBodyPos[2] - oldBodyPos[2];
	velocity = {0.0f, 0.0f, 0.0f};
	force = {0.0f, 0.0f, 0.0f};
}

void Particle::updateGridIndex() {
	gridIndex[0] = (int)((position[0] - gridMinimumPosition)/voxelSize);
	gridIndex[1] = (int)((position[1] - voxelSize)/voxelSize);
	gridIndex[2] = (int)((position[2] - gridMinimumPosition)/voxelSize);
}

int* Particle::getGridIndex() {
	return gridIndex;
}

